Aminet 6/Aminet 6 - June 1995.iso/Aminet/misc/sci/RARS_Amiga_2.lha/RARS/dynamic.cpp

C/C++ Source or Header | 1995-03-31 | 5.5 KB | 127 lines

// DYNAMIC.CPP - "driver" function for RARS - M. Timin, Feb. 1995
// This robot has no static or global variables,
// so it can drive any number of cars.
// There is a static and a global associated with passing the name,
// but the calling program expects that and handles it OK.
// adapted to ver. 0.39 3/6/95 by M. Timin

// The language here can be considered to be ANSI C or C++.

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "car.h"

// These parameters may be adjusted to get better performance:
const double CORN_SPD_CON = 6.1;   // determines how fast to take corners
const double STEER_GAIN = 2.5;     // servo gain, for staying in "lane"
const double STEER_DAMP = 1.5;     // servo damping, to prevent "weaving"
const double END_ACCEL = .27; // we accelerate until this fraction of length
const double END_CORNER = 2.5;// used to decide when to start leaving the corer
const double ESC_ALPHA = .03; // after a crash, set alpha like this, + or -
const double SLIP_LIM = 4.5;  // maximum wheel slip, ft/sec, in wheel_slip()
const double SLIP_CON = 70.0; // sets power & slip during linear acceleration

extern char* glob_name;       // The name string, below, will be copied here

// The following function calculates the speed for a corner.
// The lateral force produced by cornering is proportional to the square
// of the speed, and is inversely proportional to the radius of the path.
// Therefore, the attainable cornering speed for differnt radii is
// proportional to the square root of the radius.  This function implements
// that rule.  The value to use for CORN_SPD_CON can be determined by
// trial and error.  Example result: if the car is following a path with
// radius of 100 ft, and if CORN_SPD_CON is 5.0, then crn_spd is 50 ft/sec.
double crn_spd(double radius)
{
   if(radius < 0.0)         // change sign of negative radius
      radius = -radius;
   else if(radius == 0.0)   // This is just insurance, this funtion doesn't
      return(200.0);        // make sense when the radius is zero.
   return CORN_SPD_CON * sqrt(radius);
}

// In order to set vc, if you know how fast you want to go (goal), and how
// fast you are going now (present), This function will compute a reasonable
// value for vc.  The value is never very far from the present speed, both
// to attempt to stay within the power limit, and to maintain steering control.
// You can adjust the resulting slip by changing SLIP_LIM.
double wl_spd(double goal, double present)
{
   double ws;

   if(present > goal + 2 * SLIP_LIM)  // if too fast,
      ws = present - SLIP_LIM;      // slow down.
   else if(present < goal - 2 * SLIP_LIM)  // if too slow,
      ws = present + SLIP_LIM;             // accelerate.
   else                           // if quite close,
      ws = (goal + present) / 2;      // approach desired speed gently.

   return ws;
}

// The task of this function is to compute vc and alpha.
con_vec dynamic(situation s)
{
   const char name[] = "Dynamic";      // This is the robot driver's name!
   static int init_flag = 1;          // cleared by first call
   double speed;                      // target speed for cornering, ft/sec
   double speed_next;                 // target speed for next corner
   con_vec result;                    // This is what is returned.
   double width;                      // track width, feet
   double alpha, vc;           // components of result
   double arg;                 // for temporary storage of calculation

   // This paragraph has nothing to do with car control; it is just
   // to identify the driver by copying its name to a global RAM area:
   // This happens only on the very first call to this function
   if(init_flag)  {            // first time through, only copy name:
      strcpy(glob_name, name);
      init_flag = 0;
      result.alpha = result.vc = 0;
      return result;
   }

   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
      return result;

   // Set alpha based on a servo-mechanism approach, trying to stay
   // in the middle of the track, i.e., s.to_left equal to .5 * width:
   width = s.to_lft + s.to_rgt;                        // find width of track
   alpha = STEER_GAIN * (s.to_lft - .5 * width) / width;
   alpha -= STEER_DAMP * s.vn / s.v;  // This is damping, to prevent oscillation

   // calculate target speeds for current corner and the next:
   if(s.cur_rad == 0.0)
      arg = 0.0;
   else if(s.cur_rad > 0.0)
      arg = s.cur_rad + .5 * width;
   else
      arg = s.cur_rad - .5 * width;
   speed = crn_spd(arg);      // speed is based on radius

   if(s.nex_rad == 0.0)
      arg = 0.0;
   else if(s.nex_rad > 0.0)
      arg = s.nex_rad + .5 * width;
   else
      arg = s.nex_rad - .5 * width;
   speed_next = crn_spd(arg); // of center line of track.

   // now set the tire speed, vc:
   if(s.cur_rad == 0.0)                  // If we are on a straightaway,
      if(s.to_end > END_ACCEL * s.cur_len) // if we are far from the end,
         vc = s.v + SLIP_CON / s.v;          // keep accellerating near full power
      else                    // otherwise,
         vc = wl_spd(speed_next, s.v);    // brake for next corner
   else                              // If we're in the curve, maintain speed.
      if(s.to_end * (s.cur_rad + .5 * width) > END_CORNER * width)
                  // if we are far from the next corner, stay at "speed".
         vc = wl_spd(speed, s.v);
      else        // but when we near the next corner, adjust to "speed_next"
         vc = wl_spd(speed_next, s.v);

   result.vc = vc;   result.alpha = alpha;
   return result;
}